#include "voxel.h"

namespace Registration {

__global__ void ComputePointVoxelIndex(PointXyz *input_point_cloud,
                                       int points_num, int3 offset,
                                       int3 voxel_size, float3 voxel_scale,
                                       int thread_nums,
                                       VolumeData *volume_data) {
  int idx = blockIdx.x * blockDim.x + threadIdx.x;

  while (idx < points_num)
  {
	  int3 voxel_coord;
	  voxel_coord.x = ((input_point_cloud[idx].point.x - offset.x) / voxel_scale.x);
	  voxel_coord.y = ((input_point_cloud[idx].point.y - offset.y) / voxel_scale.y);
	  voxel_coord.z = ((input_point_cloud[idx].point.z - offset.z) / voxel_scale.z);

	  int voxel_index = voxel_coord.z * voxel_size.y * voxel_size.x +
		  voxel_coord.y * voxel_size.x + voxel_coord.x;

	  if (voxel_coord.x < voxel_size.x && voxel_coord.y < voxel_size.y &&
		  voxel_coord.z < voxel_size.z&&voxel_coord.x >= 0 && voxel_coord.y >= 0 && voxel_coord.z >= 0) {
		  int weight = volume_data[voxel_index].weight;
		  PointXyz voxel_point = volume_data[voxel_index].voxel_point;
		  volume_data[voxel_index].voxel_point.point.x = (weight*voxel_point.point.x + (input_point_cloud[idx].point.x)) / (weight + 1);
		  volume_data[voxel_index].voxel_point.point.y = (weight*voxel_point.point.y + (input_point_cloud[idx].point.y)) / (weight + 1);
		  volume_data[voxel_index].voxel_point.point.z = (weight*voxel_point.point.z + (input_point_cloud[idx].point.z)) / (weight + 1);
		  volume_data[voxel_index].isValid = true;
		  volume_data[voxel_index].weight++;
	  }
	  idx += blockDim.x*gridDim.x;
  }
  //}
}



void ComputeVoxel(PointXyz *input_point_cloud, int points_num, int3 offset,
                  int3 voxel_size, float3 voxel_scale,
                  VolumeData *volume_data) {

  int threadsPerBlock = 256;
  int blocksPerGrid =
      (points_num + threadsPerBlock - 1) / (float)(threadsPerBlock);
  Registration::ComputePointVoxelIndex<<<blocksPerGrid, threadsPerBlock>>>(
      input_point_cloud, points_num, offset, voxel_size, voxel_scale, 0,
      volume_data);
  cudaThreadSynchronize();
}

} // namespace Registration